sba.hpp File Reference

Declarations for sparse bundle adjustment related functions. More...

#include "general_resources.hpp"
#include "opencv_resources.hpp"
#include "ros_resources.hpp"
#include "reconstruction.hpp"
#include "keyframes.hpp"
#include "tracks.hpp"
#include <sba/sba.h>
#include <sba/visualization.h>
#include <time.h>

Include dependency graph for sba.hpp:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Defines
#define	DEFAULT_MAX_DISPLACEMENT   0.3
Functions
void	addPointsToSBA (SysSBA &sba, vector< cv::Point3d > &cloud)
void	addProjectionsToSBA (SysSBA &sba, vector< cv::Point2f > &loc, int idx)
void	addProjectionToSBA (SysSBA &sba, cv::Point2f &loc, unsigned int trackNo, unsigned int camNo)
double	adjustFullSystem (vector< featureTrack > &tracks, cameraParameters &camData, cv::Mat *cameras, unsigned int min_index, unsigned int max_index, unsigned int its=1)
void	assignFullSystem (SysSBA &sba, vector< featureTrack > &tracks, cameraParameters &camData, cv::Mat *cameras, unsigned int start_index, unsigned int finish_index, bool dummy=false)
void	assignPartialSystem (SysSBA &sba, vector< featureTrack > &tracks, cameraParameters &camData, cv::Mat *cameras, vector< unsigned int > &indices, bool assignProjections=true)
void	assignSystem (SysSBA &sba, vector< featureTrack > &tracks, cameraParameters &camData, cv::Mat *cameras, vector< unsigned int > &camera_indices, vector< unsigned int > &track_indices)
void	copySys (const SysSBA &src, SysSBA &dst)
double	determineSystemSize (SysSBA &sys)
	Determines distance between farthest cameras.
double	distanceBetweenPoints (Eigen::Matrix< double, 4, 1 > &pt1, Eigen::Matrix< double, 4, 1 > &pt2)
void	drawGraph2 (const SysSBA &sba, const ros::Publisher &camera_pub, const ros::Publisher &point_pub, const ros::Publisher &path_pub, int decimation=1, int bicolor=0, double scale=0.1)
void	drawKeyframes (const ros::Publisher &camera_pub, const geometry_msgs::PoseStamped *keyframePoses, unsigned int keyframeCount)
int	estimatePoseBetweenCameras (cameraParameters &camData, vector< cv::Point2f > &pts1, vector< cv::Point2f > &pts2, cv::Mat &C)
void	extractCameras (const SysSBA &sba, visualization_msgs::MarkerArray &cameraArray, visualization_msgs::Marker &marker_path)
void	extractPointCloud (const SysSBA &sba, pcl::PointCloud< pcl::PointXYZ > &point_cloud)
void	findCentroid (SysSBA &sba, cv::Point3d &centroid, cv::Point3d &stdDeviation)
void	findIntermediatePoses (vector< featureTrack > &tracks, cameraParameters &camData, cv::Mat *cameras, unsigned int image_idx_1, unsigned int image_idx_2, bool fixBothEnds=true)
void	findRelevantIndices (vector< featureTrack > &tracks, vector< unsigned int > &triangulated, vector< unsigned int > &untriangulated, unsigned int last_index, unsigned int new_index)
void	finishTracks (vector< featureTrack > &tracks, vector< cv::Point2f > &pts, double retainProp=0.80, unsigned int maxOccurrences=0)
void	getActiveCameras (cv::Mat *C, vector< unsigned int > &indices, unsigned int min_index, unsigned int max_index)
void	getActiveTracks (vector< featureTrack > &tracks, vector< unsigned int > &cameras, vector< unsigned int > &indices)
double	getFeatureMotion (vector< featureTrack > &tracks, vector< unsigned int > &indices, unsigned int idx_1, unsigned int idx_2)
void	initializeFrameCamera (frame_common::CamParams &cam_params, const cv::Mat &newCamMat, int &maxx, int &maxy, const cv::Size &cameraSize)
double	keyframeBundleAdjustment (cameraParameters &camData, vector< featureTrack > &tracks, cv::Mat *cameras, vector< unsigned int > &indices, unsigned int iterations=100, bool allFree=false, bool allFixedExceptLast=false, unsigned fixed_cams=0)
double	keyframeBundleAdjustment (cameraParameters &camData, vector< featureTrack > &tracks, keyframeStore &kf_store, cv::Mat *cameras, unsigned int kfIndex, unsigned int iterations=100)
void	normalizeSystem (SysSBA &sys, double factor=1.0)
	Scales up system size.
double	odometryBundleAdjustment (cameraParameters &camData, vector< featureTrack > &tracks, geometry_msgs::PoseStamped *keyframePoses, unsigned int keyframeCount, unsigned int iterations=100, bool debug=false)
void	optimizeFullSystem (vector< featureTrack > &tracks, cameraParameters &camData, cv::Mat *cameras, unsigned int last_index)
double	optimizeKeyframePair (vector< featureTrack > &tracks, cameraParameters &camData, int idx1, int idx2, cv::Mat *cameras)
double	optimizeSubsystem (cameraParameters &camData, cv::Mat *C, vector< unsigned int > &c_i, vector< featureTrack > &tracks, vector< unsigned int > &t_i, unsigned int iterations=100)
double	optimizeSystem (SysSBA &sba, double err=1e-3, int iterations=100, bool debug=false, int mode=SBA_SPARSE_CHOLESKY)
	*
bool	overwritePoints (const SysSBA &sys, const vector< int > &track_indices, vector< featureTrack > &tracks, double maxDisplacement=DEFAULT_MAX_DISPLACEMENT, double *averagePointShift=0, bool debug=false)
double	predictiveBundleAdjustment (cameraParameters &camData, vector< featureTrack > &tracks, geometry_msgs::PoseStamped *keyframePoses, bool *keyframeTypes, unsigned int keyframeCount, geometry_msgs::PoseStamped &newPose, unsigned int iterations=100, bool debug=false, int mode=SBA_SPARSE_CHOLESKY, double err=1e-3, int *triangulations=0, double *averagePointShift=0)
bool	reconstructFreshSubsequencePair (vector< featureTrack > &tracks, vector< cv::Point3d > &ptCloud, vector< unsigned int > &triangulatedIndices, cv::Mat &real_C0, cv::Mat &real_C1, cameraParameters camData, int idx1, int idx2)
void	removePoorTracks (vector< featureTrack > &tracks, cameraParameters &camData, cv::Mat *cameras, unsigned int start_cam, unsigned int finish_cam)
void	renormalizeSBA (SysSBA &sba, cv::Point3d &desiredCenter)
void	rescaleSBA (SysSBA &sba, unsigned int idx1, unsigned int idx2)
void	retrieveFullSystem (SysSBA &sys, cv::Mat *C, vector< featureTrack > &tracks, unsigned int start_cam, unsigned int final_cam)
void	retrievePartialSystem (SysSBA &sys, cv::Mat *C, vector< featureTrack > &tracks, vector< unsigned int > &indices)
void	retrieveSystem (SysSBA &sba, vector< featureTrack > &tracks, cameraParameters &camData, cv::Mat *cameras, vector< unsigned int > &camera_indices, vector< unsigned int > &track_indices)
void	subselectPoints (const vector< cv::Point2f > &src1, vector< cv::Point2f > &dst1, const vector< cv::Point2f > &src2, vector< cv::Point2f > &dst2)
double	testKeyframePair (vector< featureTrack > &tracks, cameraParameters &camData, double *scorecard[], int idx1, int idx2, double *scores, cv::Mat &pose, bool evaluate=false, bool debug=false)
double	twoViewBundleAdjustment (cameraParameters cam_data, cv::Mat &cam1, cv::Mat &cam2, vector< cv::Point3d > &cloud, vector< cv::Point2f > &pts1, vector< cv::Point2f > &pts2, int iterations=10)

---

Detailed Description

Declarations for sparse bundle adjustment related functions.

Definition in file sba.hpp.

---

Define Documentation

#define DEFAULT_MAX_DISPLACEMENT   0.3

Definition at line 18 of file sba.hpp.

---

Function Documentation

void addPointsToSBA	(	SysSBA &	sba,
		vector< cv::Point3d > &	cloud
	)

Definition at line 2457 of file sba.cpp.

void addProjectionsToSBA	(	SysSBA &	sba,
		vector< cv::Point2f > &	loc,
		int	idx
	)

Definition at line 2489 of file sba.cpp.

void addProjectionToSBA	(	SysSBA &	sba,
		cv::Point2f &	loc,
		unsigned int	trackNo,
		unsigned int	camNo
	)

Definition at line 2481 of file sba.cpp.

double adjustFullSystem	(	vector< featureTrack > &	tracks,
		cameraParameters &	camData,
		cv::Mat *	cameras,
		unsigned int	min_index,
		unsigned int	max_index,
		unsigned int	its = 1
	)

Definition at line 1560 of file sba.cpp.

void assignFullSystem	(	SysSBA &	sba,
		vector< featureTrack > &	tracks,
		cameraParameters &	camData,
		cv::Mat *	cameras,
		unsigned int	start_index,
		unsigned int	finish_index,
		bool	dummy = false
	)

Definition at line 1652 of file sba.cpp.

void assignPartialSystem	(	SysSBA &	sba,
		vector< featureTrack > &	tracks,
		cameraParameters &	camData,
		cv::Mat *	cameras,
		vector< unsigned int > &	indices,
		bool	assignProjections = true
	)

Definition at line 1498 of file sba.cpp.

void assignSystem	(	SysSBA &	sba,
		vector< featureTrack > &	tracks,
		cameraParameters &	camData,
		cv::Mat *	cameras,
		vector< unsigned int > &	camera_indices,
		vector< unsigned int > &	track_indices
	)

Definition at line 1606 of file sba.cpp.

void copySys	(	const SysSBA &	src,
		SysSBA &	dst
	)

Definition at line 296 of file sba.cpp.

double determineSystemSize

(

SysSBA &

sys

)

Determines distance between farthest cameras.

Definition at line 859 of file sba.cpp.

double distanceBetweenPoints	(	Eigen::Matrix< double, 4, 1 > &	pt1,
		Eigen::Matrix< double, 4, 1 > &	pt2
	)

Definition at line 845 of file sba.cpp.

void drawGraph2	(	const SysSBA &	sba,
		const ros::Publisher &	camera_pub,
		const ros::Publisher &	point_pub,
		const ros::Publisher &	path_pub,
		int	decimation = 1,
		int	bicolor = 0,
		double	scale = 0.1
	)

Definition at line 2741 of file sba.cpp.

void drawKeyframes	(	const ros::Publisher &	camera_pub,
		const geometry_msgs::PoseStamped *	keyframePoses,
		unsigned int	keyframeCount
	)

Definition at line 2618 of file sba.cpp.

int estimatePoseBetweenCameras	(	cameraParameters &	camData,
		vector< cv::Point2f > &	pts1,
		vector< cv::Point2f > &	pts2,
		cv::Mat &	C
	)

Definition at line 1928 of file sba.cpp.

void extractCameras	(	const SysSBA &	sba,
		visualization_msgs::MarkerArray &	cameraArray,
		visualization_msgs::Marker &	marker_path
	)

Definition at line 2527 of file sba.cpp.

void extractPointCloud	(	const SysSBA &	sba,
		pcl::PointCloud< pcl::PointXYZ > &	point_cloud
	)

Definition at line 2515 of file sba.cpp.

void findCentroid	(	SysSBA &	sba,
		cv::Point3d &	centroid,
		cv::Point3d &	stdDeviation
	)

Definition at line 398 of file sba.cpp.

void findIntermediatePoses	(	vector< featureTrack > &	tracks,
		cameraParameters &	camData,
		cv::Mat *	cameras,
		unsigned int	image_idx_1,
		unsigned int	image_idx_2,
		bool	fixBothEnds = true
	)

Definition at line 43 of file sba.cpp.

void findRelevantIndices	(	vector< featureTrack > &	tracks,
		vector< unsigned int > &	triangulated,
		vector< unsigned int > &	untriangulated,
		unsigned int	last_index,
		unsigned int	new_index
	)

Definition at line 7 of file sba.cpp.

void finishTracks	(	vector< featureTrack > &	tracks,
		vector< cv::Point2f > &	pts,
		double	retainProp = 0.80,
		unsigned int	maxOccurrences = 0
	)

Definition at line 3051 of file sba.cpp.

void getActiveCameras	(	cv::Mat *	C,
		vector< unsigned int > &	indices,
		unsigned int	min_index,
		unsigned int	max_index
	)

Definition at line 1293 of file sba.cpp.

void getActiveTracks	(	vector< featureTrack > &	tracks,
		vector< unsigned int > &	cameras,
		vector< unsigned int > &	indices
	)

Definition at line 1301 of file sba.cpp.

double getFeatureMotion	(	vector< featureTrack > &	tracks,
		vector< unsigned int > &	indices,
		unsigned int	idx_1,
		unsigned int	idx_2
	)

Definition at line 164 of file sba.cpp.

void initializeFrameCamera	(	frame_common::CamParams &	cam_params,
		const cv::Mat &	newCamMat,
		int &	maxx,
		int &	maxy,
		const cv::Size &	cameraSize
	)

Definition at line 2605 of file sba.cpp.

double keyframeBundleAdjustment	(	cameraParameters &	camData,
		vector< featureTrack > &	tracks,
		cv::Mat *	cameras,
		vector< unsigned int > &	indices,
		unsigned int	iterations = 100,
		bool	allFree = false,
		bool	allFixedExceptLast = false,
		unsigned	fixed_cams = 0
	)

double keyframeBundleAdjustment	(	cameraParameters &	camData,
		vector< featureTrack > &	tracks,
		keyframeStore &	kf_store,
		cv::Mat *	cameras,
		unsigned int	kfIndex,
		unsigned int	iterations = 100
	)

Definition at line 615 of file sba.cpp.

void normalizeSystem	(	SysSBA &	sys,
		double	factor = 1.0
	)

Scales up system size.

Definition at line 873 of file sba.cpp.

double odometryBundleAdjustment	(	cameraParameters &	camData,
		vector< featureTrack > &	tracks,
		geometry_msgs::PoseStamped *	keyframePoses,
		unsigned int	keyframeCount,
		unsigned int	iterations = 100,
		bool	debug = false
	)

Definition at line 1107 of file sba.cpp.

void optimizeFullSystem	(	vector< featureTrack > &	tracks,
		cameraParameters &	camData,
		cv::Mat *	cameras,
		unsigned int	last_index
	)

Definition at line 423 of file sba.cpp.

double optimizeKeyframePair	(	vector< featureTrack > &	tracks,
		cameraParameters &	camData,
		int	idx1,
		int	idx2,
		cv::Mat *	cameras
	)

Definition at line 1466 of file sba.cpp.

double optimizeSubsystem	(	cameraParameters &	camData,
		cv::Mat *	C,
		vector< unsigned int > &	c_i,
		vector< featureTrack > &	tracks,
		vector< unsigned int > &	t_i,
		unsigned int	iterations = 100
	)

Definition at line 1372 of file sba.cpp.

double optimizeSystem	(	SysSBA &	sba,
		double	err,
		int	iterations,
		bool	debug,
		int	mode
	)

*

* *

Definition at line 2354 of file sba.cpp.

bool overwritePoints	(	const SysSBA &	sys,
		const vector< int > &	track_indices,
		vector< featureTrack > &	tracks,
		double	maxDisplacement = DEFAULT_MAX_DISPLACEMENT,
		double *	averagePointShift = 0,
		bool	debug = false
	)

Definition at line 1215 of file sba.cpp.

double predictiveBundleAdjustment	(	cameraParameters &	camData,
		vector< featureTrack > &	tracks,
		geometry_msgs::PoseStamped *	keyframePoses,
		bool *	keyframeTypes,
		unsigned int	keyframeCount,
		geometry_msgs::PoseStamped &	newPose,
		unsigned int	iterations = 100,
		bool	debug = false,
		int	mode = SBA_SPARSE_CHOLESKY,
		double	err = 1e-3,
		int *	triangulations = 0,
		double *	averagePointShift = 0
	)

Definition at line 888 of file sba.cpp.

bool reconstructFreshSubsequencePair	(	vector< featureTrack > &	tracks,
		vector< cv::Point3d > &	ptCloud,
		vector< unsigned int > &	triangulatedIndices,
		cv::Mat &	real_C0,
		cv::Mat &	real_C1,
		cameraParameters	camData,
		int	idx1,
		int	idx2
	)

Definition at line 1722 of file sba.cpp.

void removePoorTracks	(	vector< featureTrack > &	tracks,
		cameraParameters &	camData,
		cv::Mat *	cameras,
		unsigned int	start_cam,
		unsigned int	finish_cam
	)

Definition at line 232 of file sba.cpp.

void renormalizeSBA	(	SysSBA &	sba,
		cv::Point3d &	desiredCenter
	)

Definition at line 335 of file sba.cpp.

void rescaleSBA	(	SysSBA &	sba,
		unsigned int	idx1,
		unsigned int	idx2
	)

Definition at line 311 of file sba.cpp.

void retrieveFullSystem	(	SysSBA &	sys,
		cv::Mat *	C,
		vector< featureTrack > &	tracks,
		unsigned int	start_cam,
		unsigned int	final_cam
	)

Definition at line 1338 of file sba.cpp.

void retrievePartialSystem	(	SysSBA &	sys,
		cv::Mat *	C,
		vector< featureTrack > &	tracks,
		vector< unsigned int > &	indices
	)

Definition at line 1258 of file sba.cpp.

void retrieveSystem	(	SysSBA &	sba,
		vector< featureTrack > &	tracks,
		cameraParameters &	camData,
		cv::Mat *	cameras,
		vector< unsigned int > &	camera_indices,
		vector< unsigned int > &	track_indices
	)

Definition at line 1593 of file sba.cpp.

void subselectPoints	(	const vector< cv::Point2f > &	src1,
		vector< cv::Point2f > &	dst1,
		const vector< cv::Point2f > &	src2,
		vector< cv::Point2f > &	dst2
	)

Definition at line 1824 of file sba.cpp.

double testKeyframePair	(	vector< featureTrack > &	tracks,
		cameraParameters &	camData,
		double *	scorecard[],
		int	idx1,
		int	idx2,
		double *	scores,
		cv::Mat &	pose,
		bool	evaluate = false,
		bool	debug = false
	)

Definition at line 1944 of file sba.cpp.

double twoViewBundleAdjustment	(	cameraParameters	cam_data,
		cv::Mat &	cam1,
		cv::Mat &	cam2,
		vector< cv::Point3d > &	cloud,
		vector< cv::Point2f > &	pts1,
		vector< cv::Point2f > &	pts2,
		int	iterations = 10
	)

Definition at line 2431 of file sba.cpp.