Classes | |
class | Con2dP2 |
class | ConP2 |
class | ConP3P |
class | ConScale |
class | CSparse |
class | CSparse2d |
class | jacobiBPCG |
Let's try templated versions. More... | |
class | JacobProds |
class | Node |
NODE holds graph nodes corresponding to frames, for use in sparse bundle adjustment. Each node has a 6DOF pose, encoded as a translation vector and rotation unit quaternion (Eigen classes). These represent the pose of the node in the world frame. More... | |
class | Node2d |
class | Proj |
Proj holds a projection measurement of a point onto a frame. They are a repository for the link between the frame and the point, with auxillary info such as Jacobians. More... | |
class | SysSBA |
SysSBA holds a set of nodes and points for sparse bundle adjustment. More... | |
class | SysSPA |
SysSPA holds a set of nodes and constraints for sparse pose adjustment. More... | |
class | SysSPA2d |
SysSPA2d holds a set of nodes and constraints for sparse pose adjustment. More... | |
class | Track |
Typedefs | |
typedef Eigen::Vector3d | Keypoint |
Keypoints - subpixel using floats. u,v are pixel coordinates, d is disparity (if stereo) More... | |
typedef Eigen::Vector4d | Point |
Point holds 3D points using in world coordinates. Currently we just represent these as 4-vectors, with a final "1.0". More... | |
typedef std::map< const int, Proj, std::less< int >, Eigen::aligned_allocator< Proj > > | ProjMap |
Functions | |
void | drawGraph (const SysSBA &sba, const ros::Publisher &camera_pub, const ros::Publisher &point_pub, int decimation=1, int bicolor=0) |
static int | getind (std::map< int, int > &m, int ind) |
int | ParseBundlerFile (const char *fin, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &camp, std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > &camR, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &camt, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &ptp, std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > &ptc, std::vector< std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > > &ptt) |
A low-level parser for bundler files. More... | |
int | ParseGraphFile (const char *fin, std::vector< Eigen::Vector5d, Eigen::aligned_allocator< Eigen::Vector5d > > &camps, std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > &camqs, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &camts, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &ptps, std::vector< std::vector< Eigen::Vector11d, Eigen::aligned_allocator< Eigen::Vector11d > > > &ptts) |
A low-level parser for graph files. More... | |
int | ParseSPAGraphFile (const char *fin, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &ntrans, std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > &nqrot, std::vector< Eigen::Vector2i, Eigen::aligned_allocator< Eigen::Vector2i > > &cind, std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &ctrans, std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > &cqrot, std::vector< Eigen::Matrix< double, 6, 6 >, Eigen::aligned_allocator< Eigen::Matrix< double, 6, 6 > > > &cvar) |
A low-level parser for graph files. More... | |
bool | read2dP2File (char *fname, SysSPA2d spa) |
constraint files More... | |
int | readBundlerFile (const char *filename, sba::SysSBA &sbaout) |
Reads bundle adjustment data from a Bundler file to an instance of SysSBA. More... | |
int | readGraphFile (const char *filename, sba::SysSBA &sbaout) |
Reads bundle adjustment data from a graph-type file to an instance of SysSBA. More... | |
bool | readP2File (char *fname, SysSPA spa) |
constraint files More... | |
int | readSPAGraphFile (const char *filename, SysSPA &spaout) |
Reads 3D pose graph data from a graph-type file to an instance of SysSPA. More... | |
void | transformF2W (Eigen::Matrix< double, 3, 4 > &m, const Eigen::Matrix< double, 4, 1 > &trans, const Eigen::Quaternion< double > &qrot) |
void | transformN2N (Eigen::Matrix< double, 4, 1 > &trans, Eigen::Quaternion< double > &qrot, Node &nd0, Node &nd1) |
void | transformW2F (Eigen::Matrix< double, 3, 4 > &m, const Eigen::Matrix< double, 4, 1 > &trans, const Eigen::Quaternion< double > &qrot) |
void | writeA (const char *fname, SysSBA &sba) |
int | writeBundlerFile (const char *filename, sba::SysSBA &sbain) |
Writes bundle adjustment data from an instance of SysSBA to a Bundler file. More... | |
int | writeGraphFile (const char *filename, SysSBA &sba, bool mono=false) |
Writes out the current SBA system as an ascii graph file suitable to be read in by the Freiburg HChol system. <mono> is true if only monocular projections are desired. More... | |
void | writeLourakisFile (const char *fname, SysSBA &sba) |
write out system in SBA form More... | |
void | writeSparseA (const char *fname, SysSBA &sba) |
typedef Eigen::Vector3d sba::Keypoint |
typedef Eigen::Vector4d sba::Point |
typedef std::map<const int, Proj, std::less<int>, Eigen::aligned_allocator<Proj> > sba::ProjMap |
void sba::drawGraph | ( | const SysSBA & | sba, |
const ros::Publisher & | camera_pub, | ||
const ros::Publisher & | point_pub, | ||
int | decimation = 1 , |
||
int | bicolor = 0 |
||
) |
Definition at line 5 of file visualization.cpp.
|
inlinestatic |
Run the LM algorithm that computes a nonlinear SPA estimate. <window> is the number of nodes in the window, the last nodes added <niter> is the max number of iterations to perform; returns the number actually performed. <lambda> is the diagonal augmentation for LM.
<useCSParse> = 0 for dense Cholesky, 1 for sparse Cholesky, 2 for sparse PCG
int sba::ParseBundlerFile | ( | const char * | fin, |
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > & | camp, | ||
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > & | camR, | ||
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > & | camt, | ||
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > & | ptp, | ||
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > & | ptc, | ||
std::vector< std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > > & | ptt | ||
) |
A low-level parser for bundler files.
int sba::ParseGraphFile | ( | const char * | fin, |
std::vector< Eigen::Vector5d, Eigen::aligned_allocator< Eigen::Vector5d > > & | camps, | ||
std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > & | camqs, | ||
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > & | camts, | ||
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > & | ptps, | ||
std::vector< std::vector< Eigen::Vector11d, Eigen::aligned_allocator< Eigen::Vector11d > > > & | ptts | ||
) |
A low-level parser for graph files.
int sba::ParseSPAGraphFile | ( | const char * | fin, |
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > & | ntrans, | ||
std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > & | nqrot, | ||
std::vector< Eigen::Vector2i, Eigen::aligned_allocator< Eigen::Vector2i > > & | cind, | ||
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > & | ctrans, | ||
std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > & | cqrot, | ||
std::vector< Eigen::Matrix< double, 6, 6 >, Eigen::aligned_allocator< Eigen::Matrix< double, 6, 6 > > > & | cvar | ||
) |
A low-level parser for graph files.
Definition at line 996 of file sba_file_io.cpp.
bool sba::read2dP2File | ( | char * | fname, |
SysSPA2d | spa | ||
) |
constraint files
reads in a file of pose constraints
int sba::readBundlerFile | ( | const char * | filename, |
sba::SysSBA & | sbaout | ||
) |
Reads bundle adjustment data from a Bundler file to an instance of SysSBA.
filename | The name of the bundler-formatted file to read from. |
sbaout | An instance of SBA that the file will be written to. It should be noted that the bundler format does not support stereo points; all points read in will be monocular. Note: documentation of the Bundler format can be found at http://phototour.cs.washington.edu/bundler/bundler-v0.3-manual.html . |
Definition at line 9 of file sba_file_io.cpp.
int sba::readGraphFile | ( | const char * | filename, |
sba::SysSBA & | sbaout | ||
) |
Reads bundle adjustment data from a graph-type file to an instance of SysSBA.
filename | The name of the bundler-formatted file to read from. |
sbaout | An instance of SBA that the file will be written to. Note: where is the documentation for this format? |
Definition at line 488 of file sba_file_io.cpp.
bool sba::readP2File | ( | char * | fname, |
SysSPA | spa | ||
) |
int sba::readSPAGraphFile | ( | const char * | filename, |
SysSPA & | spaout | ||
) |
Reads 3D pose graph data from a graph-type file to an instance of SysSPA.
filename | The name of the bundler-formatted file to read from. |
spaout | An instance of SPA that the file will be written to. Note: where is the documentation for this format? |
Definition at line 929 of file sba_file_io.cpp.
void sba::transformF2W | ( | Eigen::Matrix< double, 3, 4 > & | m, |
const Eigen::Matrix< double, 4, 1 > & | trans, | ||
const Eigen::Quaternion< double > & | qrot | ||
) |
void sba::transformW2F | ( | Eigen::Matrix< double, 3, 4 > & | m, |
const Eigen::Matrix< double, 4, 1 > & | trans, | ||
const Eigen::Quaternion< double > & | qrot | ||
) |
void sba::writeA | ( | const char * | fname, |
SysSBA & | sba | ||
) |
Definition at line 401 of file sba_file_io.cpp.
int sba::writeBundlerFile | ( | const char * | filename, |
sba::SysSBA & | sbain | ||
) |
Writes bundle adjustment data from an instance of SysSBA to a Bundler file.
filename | The name of the bundler-formatted file to write to. The file is created, or if it already exists, truncated. |
sbain | An instance of SBA that the file will be written from. It should be noted that the bundler format does not support stereo points; all points written will be monocular. Also, since SBA does not store point color information, all points will be colored white. Note: documentation of the Bundler format can be found at http://phototour.cs.washington.edu/bundler/bundler-v0.3-manual.html . |
Definition at line 114 of file sba_file_io.cpp.
int sba::writeGraphFile | ( | const char * | filename, |
SysSBA & | sba, | ||
bool | mono = false |
||
) |
Writes out the current SBA system as an ascii graph file suitable to be read in by the Freiburg HChol system. <mono> is true if only monocular projections are desired.
Writes out the current SBA system as an ascii graph file suitable to be read in by the Freiburg HChol system.
<mono> is true if only monocular projections are desired.
Definition at line 783 of file sba_file_io.cpp.
void sba::writeLourakisFile | ( | const char * | fname, |
SysSBA & | sba | ||
) |
write out system in SBA form
Definition at line 322 of file sba_file_io.cpp.
void sba::writeSparseA | ( | const char * | fname, |
SysSBA & | sba | ||
) |
Definition at line 418 of file sba_file_io.cpp.