118 lines
3.2 KiB
C++
118 lines
3.2 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 {
|
|
|
|
/* ************************************************************************* */
|
|
// Exponential map
|
|
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;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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, double tol) const {
|
|
for (PoseMap::const_iterator it = cameraIteratorBegin(); it
|
|
!= cameraIteratorEnd(); it++) {
|
|
if (!c.cameraPoseExists(it->first)) return false;
|
|
if (!it->second.equals(c.cameraPose(it->first), tol)) return false;
|
|
}
|
|
|
|
for (PointMap::const_iterator it = landmarkIteratorBegin(); it
|
|
!= landmarkIteratorEnd(); it++) {
|
|
if (!c.landmarkPointExists(it->first)) return false;
|
|
if (!it->second.equals(c.landmarkPoint(it->first), tol)) return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
} // namespace gtsam
|
|
|