12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

templated version for Kitti, triangulation, and factorCreators

release/4.3a0
Luca Carlone 2013-10-15 22:43:10 +00:00
parent 6969b942b6
commit 01f6ee56e4
6 changed files with 121 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp
View File

@ -281,7 +281,7 @@ int main(int argc, char** argv) {
unsigned int maxNumPoses = 1e+6;
// Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used
bool useSmartProjectionFactor = false;
bool useSmartProjectionFactor = true;
bool useLM = true;
double KittiLinThreshold = -1.0; // 0.005; //

40
gtsam_unstable/examples/SmartProjectionFactorTesting.cpp
View File

@ -62,6 +62,8 @@ using symbol_shorthand::X;
using symbol_shorthand::L;
typedef PriorFactor<Pose3> Pose3Prior;
//typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> SmartFactorsCreator;
//typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> ProjectionFactorsCreator;
typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3_S2> SmartFactorsCreator;
typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3_S2> ProjectionFactorsCreator;
typedef FastMap<Key, int> OrderingMap;
@ -199,12 +201,10 @@ void optimizeGraphISAM2(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr g
// main
int main(int argc, char** argv) {
// unsigned int maxNumLandmarks = 1e+7;
// unsigned int maxNumPoses = 1e+7;
// Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used
bool useSmartProjectionFactor = false;
bool useSmartProjectionFactor = true;
bool useLM = true;
bool addNoise = false;
// Smart factors settings
double linThreshold = -1.0; // negative is disabled
@ -216,6 +216,8 @@ int main(int argc, char** argv) {
std::cout << "PARAM SmartFactor: " << useSmartProjectionFactor << std::endl;
std::cout << "PARAM LM: " << useLM << std::endl;
std::cout << "PARAM linThreshold (negative is disabled): " << linThreshold << std::endl;
if(addNoise)
std::cout << "PARAM Noise: " << addNoise << std::endl;
// Get home directory and dataset
string HOME = getenv("HOME");
@ -249,10 +251,10 @@ int main(int argc, char** argv) {
bool optimized = false;
boost::shared_ptr<Ordering> ordering(new Ordering());
// std::vector< boost::shared_ptr<Cal3Bundler> > K_bundler_cameras; // TODO: uncomment
std::vector< boost::shared_ptr<Cal3_S2> > K_S2_cameras;
// std::vector< boost::shared_ptr<Cal3Bundler> > K_cameras; // TODO: uncomment
std::vector< boost::shared_ptr<Cal3_S2> > K_cameras;
// boost::shared_ptr<Cal3Bundler> Kbund(new Cal3Bundler());// TODO: uncomment
// boost::shared_ptr<Cal3Bundler> K(new Cal3Bundler()); // TODO: uncomment
Cal3_S2::shared_ptr K(new Cal3_S2());
SmartFactorsCreator smartCreator(pixel_sigma, K, rankTolerance, linThreshold); // this initial K is not used
@ -287,14 +289,23 @@ int main(int argc, char** argv) {
for(unsigned int i = 0; i < totNumPoses; i++){
// R,t,f,k1 and k2.
fin >> rotx >> roty >> rotz >> x >> y >> z >> f >> k1 >> k2;
// boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(f, k1, k2, 0.0, 0.0)); // TODO: uncomment
// K_cameras.push_back(Kbundler); // TODO: uncomment
// boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(f, k1, k2, 0.0, 0.0)); //TODO: uncomment
// K_cameras.push_back(Kbundler); //TODO: uncomment
boost::shared_ptr<Cal3_S2> K_S2(new Cal3_S2(f, f, 0.0, 0.0, 0.0));
K_S2_cameras.push_back(K_S2);
K_cameras.push_back(K_S2);
Vector3 rotVect(rotx,roty,rotz);
// FORMAT CONVERSION!! R -> R'
Rot3 R = Rot3::Expmap(rotVect);
loadedValues->insert(X(i), Pose3(R.inverse(), - R.unrotate(Point3(x,y,z)) ) );
Matrix3 R_bal_gtsam_mat = Matrix3::Zero(3,3);
R_bal_gtsam_mat(0,0) = 1.0; R_bal_gtsam_mat(1,1) = -1.0; R_bal_gtsam_mat(2,2) = -1.0;
Rot3 R_bal_gtsam_ = Rot3(R_bal_gtsam_mat);
Pose3 CameraPose((R.inverse()).compose(R_bal_gtsam_), - R.unrotate(Point3(x,y,z)));
if(addNoise){
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.3,0.1,0.3));
CameraPose = CameraPose.compose(noise_pose);
}
loadedValues->insert(X(i), CameraPose );
}
cout << "last pose: " << x << " " << y << " " << z << " " << rotx << " "
@ -319,7 +330,7 @@ int main(int argc, char** argv) {
l = vector_l.at(i);
r = vector_r.at(i);
// FORMAT CONVERSION!! XPOINT
u = - vector_u.at(i);
u = vector_u.at(i);
// FORMAT CONVERSION!! XPOINT
v = - vector_v.at(i);
@ -337,7 +348,7 @@ int main(int argc, char** argv) {
} else {
// or STANDARD PROJECTION FACTORS
projectionCreator.add(L(l), X(r), Point2(u,v), pixel_sigma, K_S2_cameras.at(r), graphProjection);
projectionCreator.add(L(l), X(r), Point2(u,v), pixel_sigma, K_cameras.at(r), graphProjection);
numLandmarks = projectionCreator.getNumLandmarks();
optimized = false;
}
@ -382,6 +393,9 @@ int main(int argc, char** argv) {
optimized = true;
cout << "===================================================" << endl;
tictoc_print_();
cout << "===================================================" << endl;
writeValues("./", result);
// if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);

27
gtsam_unstable/geometry/triangulation.cpp
View File

@ -54,33 +54,6 @@ Point3 triangulateDLT(const vector<Matrix>& projection_matrices,
return Point3(sub( (v / v(3)),0,3));
}
/* ************************************************************************* */
Point3 triangulatePoint3(const vector<Pose3>& poses,
const vector<Point2>& measurements, const Cal3_S2& K, double rank_tol) {
assert(poses.size() == measurements.size());
if(poses.size() < 2)
throw(TriangulationUnderconstrainedException());
vector<Matrix> projection_matrices;
// construct projection matrices from poses & calibration
BOOST_FOREACH(const Pose3& pose, poses)
projection_matrices += K.matrix() * sub(pose.inverse().matrix(),0,3,0,4);
Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol);
// verify that the triangulated point lies infront of all cameras
BOOST_FOREACH(const Pose3& pose, poses) {
const Point3& p_local = pose.transform_to(triangulated_point);
if(p_local.z() <= 0)
throw(TriangulationCheiralityException());
}
return triangulated_point;
}
/* ************************************************************************* */
} // namespace gtsam

33
gtsam_unstable/geometry/triangulation.h
View File

@ -47,6 +47,9 @@ public:
}
};
Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol);
/**
* Function to triangulate 3D landmark point from an arbitrary number
* of poses (at least 2) using the DLT. The function checks that the
@ -58,12 +61,32 @@ public:
* @param rank tolerance, default 1e-9
* @return Returns a Point3 on success, boost::none otherwise.
*/
template<class CALIBRATION>
GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector<Pose3>& poses,
const std::vector<Point2>& measurements, const Cal3_S2& K, double rank_tol =
1e-9);
const std::vector<Point2>& measurements, const CALIBRATION& K, double rank_tol = 1e-9){
Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol);
assert(poses.size() == measurements.size());
if(poses.size() < 2)
throw(TriangulationUnderconstrainedException());
std::vector<Matrix> projection_matrices;
// construct projection matrices from poses & calibration
BOOST_FOREACH(const Pose3& pose, poses)
projection_matrices.push_back( K.K() * sub(pose.inverse().matrix(),0,3,0,4) );
Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol);
// verify that the triangulated point lies infront of all cameras
BOOST_FOREACH(const Pose3& pose, poses) {
const Point3& p_local = pose.transform_to(triangulated_point);
if(p_local.z() <= 0)
throw(TriangulationCheiralityException());
}
return triangulated_point;
}
/* ************************************************************************* */
template<class CALIBRATION>
@ -80,7 +103,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
// construct projection matrices from poses & calibration
for(size_t i = 0; i<poses.size(); i++){
projection_matrices.push_back( Ks.at(i)->matrix() * sub(poses.at(i).inverse().matrix(),0,3,0,4) );
projection_matrices.push_back( Ks.at(i)->K() * sub(poses.at(i).inverse().matrix(),0,3,0,4) );
}
Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol);

24
gtsam_unstable/slam/GenericProjectionFactorsCreator.h
View File

@ -31,12 +31,14 @@
namespace gtsam {
typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> ProjectionFactor;
typedef FastMap<Key, std::vector<boost::shared_ptr<ProjectionFactor> > > ProjectionFactorMap;
typedef FastMap<Key, int> OrderingMap;
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class GenericProjectionFactorsCreator {
typedef GenericProjectionFactor<Pose3, Point3, CALIBRATION> ProjectionFactor;
typedef FastMap<Key, std::vector<boost::shared_ptr<ProjectionFactor> > > ProjectionFactorMap;
typedef FastMap<Key, int> OrderingMap;
public:
GenericProjectionFactorsCreator(const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& K,
@ -51,10 +53,10 @@ namespace gtsam {
bool debug = false;
// Create projection factor
ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(measurement, noise_, poseKey, landmarkKey, K_));
boost::shared_ptr<ProjectionFactor> projectionFactor(new ProjectionFactor(measurement, noise_, poseKey, landmarkKey, K_));
// Check if landmark exists in mapping
ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey);
typename ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey);
if (pfit != projectionFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
@ -65,7 +67,7 @@ namespace gtsam {
if (debug) fprintf(stderr,"New landmark (%d)\n", pfit != projectionFactors.end());
// Create a new vector of projection factors
std::vector<ProjectionFactor::shared_ptr> projectionFactorVector;
std::vector<boost::shared_ptr<ProjectionFactor> > projectionFactorVector;
projectionFactorVector.push_back(projectionFactor);
// Insert projection factor to NEW list of projection factors associated with this landmark
@ -89,10 +91,10 @@ namespace gtsam {
bool debug = false;
// Create projection factor
ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(measurement, model, poseKey, landmarkKey, K));
typename ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(measurement, model, poseKey, landmarkKey, K));
// Check if landmark exists in mapping
ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey);
typename ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey);
if (pfit != projectionFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
@ -103,7 +105,7 @@ namespace gtsam {
if (debug) fprintf(stderr,"New landmark (%d)\n", pfit != projectionFactors.end());
// Create a new vector of projection factors
std::vector<ProjectionFactor::shared_ptr> projectionFactorVector;
std::vector<boost::shared_ptr<ProjectionFactor> > projectionFactorVector;
projectionFactorVector.push_back(projectionFactor);
// Insert projection factor to NEW list of projection factors associated with this landmark
@ -166,11 +168,11 @@ namespace gtsam {
std::cout << "Reading initial guess for 3D points from file" << std::endl;
std::vector<boost::shared_ptr<ProjectionFactor> > projectionFactorVector;
std::vector<boost::shared_ptr<ProjectionFactor> >::iterator vfit;
typename std::vector<boost::shared_ptr<ProjectionFactor> >::iterator vfit;
Point3 point;
Pose3 cameraPose;
ProjectionFactorMap::iterator pfit;
typename ProjectionFactorMap::iterator pfit;
if (debug) graphValues->print("graphValues \n");
if (debug) std::cout << " # END VALUES: " << std::endl;

61
gtsam_unstable/slam/SmartProjectionFactorsCreator.h
View File

@ -14,7 +14,8 @@
// Use a map to store landmark/smart factor pairs
#include <gtsam/base/FastMap.h>
#include <gtsam_unstable/slam/SmartProjectionFactor.h>
// #include <gtsam_unstable/slam/SmartProjectionFactor.h>
#include <gtsam_unstable/slam/SmartProjectionHessianFactor.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
@ -25,12 +26,13 @@
namespace gtsam {
typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
typedef FastMap<Key, boost::shared_ptr<SmartProjectionFactorState> > SmartFactorToStateMap;
typedef FastMap<Key, boost::shared_ptr<SmartFactor> > SmartFactorMap;
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class SmartProjectionFactorsCreator {
typedef SmartProjectionHessianFactor<Pose3, Point3, CALIBRATION> SmartFactor;
typedef FastMap<Key, boost::shared_ptr<SmartProjectionHessianFactorState> > SmartFactorToStateMap;
typedef FastMap<Key, boost::shared_ptr<SmartFactor> > SmartFactorMap;
public:
SmartProjectionFactorsCreator(const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& K,
@ -51,12 +53,12 @@ namespace gtsam {
// Check if landmark exists in mapping
SmartFactorToStateMap::iterator fsit = smartFactorStates.find(landmarkKey);
SmartFactorMap::iterator fit = smartFactors.find(landmarkKey);
typename SmartFactorMap::iterator fit = smartFactors.find(landmarkKey);
if (fsit != smartFactorStates.end() && fit != smartFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// Add measurement to smart factor
(*fit).second->add(measurement, poseKey);
(*fit).second->add(measurement, poseKey, noise_, K_);
totalNumMeasurements++;
if (debug) (*fit).second->print();
@ -68,8 +70,9 @@ namespace gtsam {
measurements.push_back(measurement);
// This is a new landmark, create a new factor and add to mapping
boost::shared_ptr<SmartProjectionFactorState> smartFactorState(new SmartProjectionFactorState());
SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, noise_, K_, rankTolerance_, linearizationThreshold_));
boost::shared_ptr<SmartProjectionHessianFactorState> smartFactorState(new SmartProjectionHessianFactorState());
boost::shared_ptr<SmartFactor> smartFactor(new SmartFactor(rankTolerance_, linearizationThreshold_));
smartFactor->add(measurement, poseKey, noise_, K_);
smartFactorStates.insert( std::make_pair(landmarkKey, smartFactorState) );
smartFactors.insert( std::make_pair(landmarkKey, smartFactor) );
graph.push_back(smartFactor);
@ -83,6 +86,46 @@ namespace gtsam {
}
}
void add(Key landmarkKey, Key poseKey,
Point2 measurement,
const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& K,
NonlinearFactorGraph &graph) {
std::vector<Key> views;
std::vector<Point2> measurements;
bool debug = false;
// Check if landmark exists in mapping
SmartFactorToStateMap::iterator fsit = smartFactorStates.find(landmarkKey);
typename SmartFactorMap::iterator fit = smartFactors.find(landmarkKey);
if (fsit != smartFactorStates.end() && fit != smartFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// Add measurement to smart factor
(*fit).second->add(measurement, poseKey, model, K);
totalNumMeasurements++;
if (debug) (*fit).second->print();
} else {
if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), fit != smartFactors.end());
// This is a new landmark, create a new factor and add to mapping
boost::shared_ptr<SmartProjectionHessianFactorState> smartFactorState(new SmartProjectionHessianFactorState());
boost::shared_ptr<SmartFactor> smartFactor(new SmartFactor(rankTolerance_, linearizationThreshold_));
smartFactor->add(measurement, poseKey, model, K);
smartFactorStates.insert( std::make_pair(landmarkKey, smartFactorState) );
smartFactors.insert( std::make_pair(landmarkKey, smartFactor) );
graph.push_back(smartFactor);
numLandmarks++;
totalNumMeasurements++;
}
}
unsigned int getTotalNumMeasurements() { return totalNumMeasurements; }
unsigned int getNumLandmarks() { return numLandmarks; }