Added try/catch for triangulation exception
Optimizations resulting in about 6-7% improvement Added methods returning keys and other information needed to create generic projection factors Code cleanuprelease/4.3a0
Zsolt Kira
parent
0bc208e093
commit
0466e606b0
|
|
@ -171,8 +171,13 @@ namespace gtsam {
|
||||||
/// linearize returns a Hessianfactor that is an approximation of error(p)
|
/// linearize returns a Hessianfactor that is an approximation of error(p)
|
||||||
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
|
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
|
||||||
|
|
||||||
bool debug = false;
|
bool blockwise = false;
|
||||||
bool blockwise = true;
|
|
||||||
|
unsigned int numKeys = keys_.size();
|
||||||
|
std::vector<Index> js;
|
||||||
|
std::vector<Matrix> Gs(numKeys*(numKeys+1)/2);
|
||||||
|
std::vector<Vector> gs(numKeys);
|
||||||
|
double f=0;
|
||||||
|
|
||||||
// Collect all poses (Cameras)
|
// Collect all poses (Cameras)
|
||||||
std::vector<Pose3> cameraPoses;
|
std::vector<Pose3> cameraPoses;
|
||||||
|
|
@ -184,53 +189,25 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
|
|
||||||
// We triangulate the 3D position of the landmark
|
// We triangulate the 3D position of the landmark
|
||||||
if (debug) {
|
|
||||||
BOOST_FOREACH(const Pose3& pose, cameraPoses) {
|
|
||||||
std::cout << "Pose: " << pose << std::endl;
|
|
||||||
}
|
|
||||||
BOOST_FOREACH(const Point2& point, measured_) {
|
|
||||||
std::cout << "Point: " << point << std::endl;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
boost::optional<Point3> point;
|
boost::optional<Point3> point;
|
||||||
if (point_) {
|
try {
|
||||||
point = point_;
|
point = triangulatePoint3(cameraPoses, measured_, *K_);
|
||||||
//std::cout << "Using existing point " << *point << std::endl;
|
} catch( TriangulationCheiralityException& e) {
|
||||||
} else {
|
// point is behind one of the cameras, turn factor off by setting everything to 0
|
||||||
//std::cout << "Triangulating in linearize " << std::endl;
|
//std::cout << e.what() << std::end;
|
||||||
point = triangulatePoint3(cameraPoses, measured_, *K_);
|
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
|
||||||
}
|
BOOST_FOREACH(Vector& v, gs) v = zero(6);
|
||||||
if (debug) std::cout << "Result: " << *point << std::endl;
|
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
|
||||||
|
|
||||||
|
|
||||||
if (debug) {
|
|
||||||
std::cout << "point " << *point << std::endl;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<Matrix> Gs(keys_.size()*(keys_.size()+1)/2);
|
if (blockwise){
|
||||||
std::vector<Vector> gs(keys_.size());
|
|
||||||
double f=0;
|
|
||||||
|
|
||||||
// point is behind one of the cameras, turn factor off by setting everything to 0
|
|
||||||
if (!point) {
|
|
||||||
std::cout << "WARNING: Could not triangulate during linearize" << std::endl;
|
|
||||||
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6,6);
|
|
||||||
BOOST_FOREACH(Vector& v, gs) v = zero(6);
|
|
||||||
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
|
|
||||||
}
|
|
||||||
|
|
||||||
// For debug only
|
|
||||||
std::vector<Matrix> Gs1;
|
|
||||||
std::vector<Vector> gs1;
|
|
||||||
if (blockwise || debug){
|
|
||||||
// ==========================================================================================================
|
// ==========================================================================================================
|
||||||
std::vector<Matrix> Hx(keys_.size());
|
std::vector<Matrix> Hx(numKeys);
|
||||||
std::vector<Matrix> Hl(keys_.size());
|
std::vector<Matrix> Hl(numKeys);
|
||||||
std::vector<Vector> b(keys_.size());
|
std::vector<Vector> b(numKeys);
|
||||||
|
|
||||||
for(size_t i = 0; i < measured_.size(); i++) {
|
for(size_t i = 0; i < measured_.size(); i++) {
|
||||||
Pose3 pose = cameraPoses.at(i);
|
Pose3 pose = cameraPoses.at(i);
|
||||||
// std::cout << "pose " << pose << std::endl;
|
|
||||||
PinholeCamera<CALIBRATION> camera(pose, *K_);
|
PinholeCamera<CALIBRATION> camera(pose, *K_);
|
||||||
b.at(i) = - ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
|
b.at(i) = - ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
|
||||||
noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i));
|
noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i));
|
||||||
|
|
@ -255,64 +232,33 @@ namespace gtsam {
|
||||||
for(size_t i2 = 0; i2 < keys_.size(); i2++) {
|
for(size_t i2 = 0; i2 < keys_.size(); i2++) {
|
||||||
// we only need the upper triangular entries
|
// we only need the upper triangular entries
|
||||||
Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose();
|
Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose();
|
||||||
if (i1==0 && i2==0){
|
|
||||||
if (debug) {
|
|
||||||
std::cout << "Hoff"<< i1 << i2 << "=[" << Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose() << "];" << std::endl;
|
|
||||||
std::cout << "Hxoff"<< "=[" << Hx.at(i1) << "];" << std::endl;
|
|
||||||
std::cout << "Hloff"<< "=[" << Hl.at(i1) << "];" << std::endl;
|
|
||||||
std::cout << "Hloff2"<< "=[" << Hl.at(i2) << "];" << std::endl;
|
|
||||||
std::cout << "C"<< "=[" << C << "];" << std::endl;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// Populate Gs and gs
|
// Populate Gs and gs
|
||||||
int GsCount = 0;
|
int GsCount = 0;
|
||||||
for(size_t i1 = 0; i1 < keys_.size(); i1++) {
|
for(size_t i1 = 0; i1 < numKeys; i1++) {
|
||||||
gs.at(i1) = Hx.at(i1).transpose() * b.at(i1);
|
gs.at(i1) = Hx.at(i1).transpose() * b.at(i1);
|
||||||
|
|
||||||
for(size_t i2 = 0; i2 < keys_.size(); i2++) {
|
for(size_t i2 = 0; i2 < numKeys; i2++) {
|
||||||
gs.at(i1) -= Hxl[i1][i2] * b.at(i2);
|
gs.at(i1) -= Hxl[i1][i2] * b.at(i2);
|
||||||
|
|
||||||
if (i2 == i1){
|
if (i2 == i1){
|
||||||
Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2);
|
Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2);
|
||||||
|
|
||||||
if (debug) {
|
|
||||||
std::cout << "HxlH"<< GsCount << "=[" << Hxl[i1][i2] * Hx.at(i2) << "];" << std::endl;
|
|
||||||
std::cout << "Hx2_"<< GsCount << "=[" << Hx.at(i2) << "];" << std::endl;
|
|
||||||
std::cout << "H"<< GsCount << "=[" << Gs.at(GsCount) << "];" << std::endl;
|
|
||||||
}
|
|
||||||
GsCount++;
|
GsCount++;
|
||||||
}
|
}
|
||||||
if (i2 > i1) {
|
if (i2 > i1) {
|
||||||
Gs.at(GsCount) = - Hxl[i1][i2] * Hx.at(i2);
|
Gs.at(GsCount) = - Hxl[i1][i2] * Hx.at(i2);
|
||||||
|
|
||||||
if (debug) {
|
|
||||||
std::cout << "HxlH"<< GsCount << "=[" << Hxl[i1][i2] * Hx.at(i2) << "];" << std::endl;
|
|
||||||
std::cout << "Hx2_"<< GsCount << "=[" << Hx.at(i2) << "];" << std::endl;
|
|
||||||
std::cout << "H"<< GsCount << "=[" << Gs.at(GsCount) << "];" << std::endl;
|
|
||||||
}
|
|
||||||
GsCount++;
|
GsCount++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (debug) {
|
|
||||||
// Copy result for later comparison
|
|
||||||
BOOST_FOREACH(const Matrix& m, Gs) {
|
|
||||||
Gs1.push_back(m);
|
|
||||||
}
|
|
||||||
// Copy result for later comparison
|
|
||||||
BOOST_FOREACH(const Matrix& m, gs) {
|
|
||||||
gs1.push_back(m);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (blockwise == false || debug){ // version with full matrix multiplication
|
if (blockwise == false){ // version with full matrix multiplication
|
||||||
// ==========================================================================================================
|
// ==========================================================================================================
|
||||||
Matrix Hx2 = zeros(2*keys_.size(), 6*keys_.size());
|
Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys);
|
||||||
Matrix Hl2 = zeros(2*keys_.size(), 3);
|
Matrix Hl2 = zeros(2 * numKeys, 3);
|
||||||
Vector b2 = zero(2*keys_.size());
|
Vector b2 = zero(2 * numKeys);
|
||||||
|
|
||||||
for(size_t i = 0; i < measured_.size(); i++) {
|
for(size_t i = 0; i < measured_.size(); i++) {
|
||||||
Pose3 pose = cameraPoses.at(i);
|
Pose3 pose = cameraPoses.at(i);
|
||||||
|
|
@ -326,39 +272,24 @@ namespace gtsam {
|
||||||
Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
|
Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
|
||||||
Hl2.block( 2*i, 0, 2, 3 ) = Hli;
|
Hl2.block( 2*i, 0, 2, 3 ) = Hli;
|
||||||
|
|
||||||
if (debug) {
|
|
||||||
std::cout << "Hxi= \n" << Hxi << std::endl;
|
|
||||||
std::cout << "Hxi.transpose() * Hxi= \n" << Hxi.transpose() * Hxi << std::endl;
|
|
||||||
std::cout << "Hxl.transpose() * Hxl= \n" << Hli.transpose() * Hli << std::endl;
|
|
||||||
}
|
|
||||||
subInsert(b2,bi,2*i);
|
subInsert(b2,bi,2*i);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Shur complement trick
|
// Shur complement trick
|
||||||
Matrix H(6*keys_.size(), 6*keys_.size());
|
Matrix H(6 * numKeys, 6 * numKeys);
|
||||||
Matrix3 C2 = (Hl2.transpose() * Hl2).inverse();
|
Matrix3 C2 = (Hl2.transpose() * Hl2).inverse();
|
||||||
H = Hx2.transpose() * Hx2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * Hx2;
|
H = Hx2.transpose() * (Hx2 - (Hl2 * (C2 * (Hl2.transpose() * Hx2))));
|
||||||
|
|
||||||
if (debug) {
|
Vector gs_vector = Hx2.transpose() * (b2 - (Hl2 * (C2 * (Hl2.transpose() * b2))));
|
||||||
std::cout << "Hx2" << "=[" << Hx2 << "];" << std::endl;
|
|
||||||
std::cout << "Hl2" << "=[" << Hl2 << "];" << std::endl;
|
|
||||||
std::cout << "H" << "=[" << H << "];" << std::endl;
|
|
||||||
|
|
||||||
std::cout << "Cnoinv2"<< "=[" << Hl2.transpose() * Hl2 << "];" << std::endl;
|
|
||||||
std::cout << "C2"<< "=[" << C2 << "];" << std::endl;
|
|
||||||
std::cout << "================================================================================" << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
Vector gs_vector = Hx2.transpose() * b2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * b2;
|
|
||||||
|
|
||||||
|
|
||||||
// Populate Gs and gs
|
// Populate Gs and gs
|
||||||
int GsCount2 = 0;
|
int GsCount2 = 0;
|
||||||
for(size_t i1 = 0; i1 < keys_.size(); i1++) {
|
for(size_t i1 = 0; i1 < numKeys; i1++) {
|
||||||
gs.at(i1) = sub(gs_vector, 6*i1, 6*i1 + 6);
|
gs.at(i1) = sub(gs_vector, 6*i1, 6*i1 + 6);
|
||||||
|
|
||||||
for(size_t i2 = 0; i2 < keys_.size(); i2++) {
|
for(size_t i2 = 0; i2 < numKeys; i2++) {
|
||||||
if (i2 >= i1) {
|
if (i2 >= i1) {
|
||||||
Gs.at(GsCount2) = H.block(6*i1, 6*i2, 6, 6);
|
Gs.at(GsCount2) = H.block(6*i1, 6*i2, 6, 6);
|
||||||
GsCount2++;
|
GsCount2++;
|
||||||
|
|
@ -368,27 +299,6 @@ namespace gtsam {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (debug) {
|
|
||||||
// Compare blockwise and full version
|
|
||||||
bool gs1_equal_gs = true;
|
|
||||||
for(size_t i = 0; i < measured_.size(); i++) {
|
|
||||||
std::cout << "gs.at(i) " << gs.at(i).transpose() << std::endl;
|
|
||||||
std::cout << "gs1.at(i) " << gs1.at(i).transpose() << std::endl;
|
|
||||||
std::cout << "gs.error " << (gs.at(i)- gs1.at(i)).transpose() << std::endl;
|
|
||||||
if( !equal(gs.at(i), gs1.at(i)), 1e-7) {
|
|
||||||
gs1_equal_gs = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
std::cout << "gs1_equal_gs " << gs1_equal_gs << std::endl;
|
|
||||||
|
|
||||||
for(size_t i = 0; i < keys_.size()*(keys_.size()+1)/2; i++) {
|
|
||||||
std::cout << "Gs.at(i) " << Gs.at(i).transpose() << std::endl;
|
|
||||||
std::cout << "Gs1.at(i) " << Gs1.at(i).transpose() << std::endl;
|
|
||||||
std::cout << "Gs.error " << (Gs.at(i)- Gs1.at(i)).transpose() << std::endl;
|
|
||||||
}
|
|
||||||
std::cout << "Gs1_equal_Gs " << gs1_equal_gs << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
// ==========================================================================================================
|
// ==========================================================================================================
|
||||||
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
|
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
|
||||||
}
|
}
|
||||||
|
|
@ -400,7 +310,6 @@ namespace gtsam {
|
||||||
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
|
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
|
||||||
*/
|
*/
|
||||||
virtual double error(const Values& values) const {
|
virtual double error(const Values& values) const {
|
||||||
bool debug = false;
|
|
||||||
if (this->active(values)) {
|
if (this->active(values)) {
|
||||||
double overallError=0;
|
double overallError=0;
|
||||||
|
|
||||||
|
|
@ -415,39 +324,23 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
|
|
||||||
// We triangulate the 3D position of the landmark
|
// We triangulate the 3D position of the landmark
|
||||||
if (debug) {
|
|
||||||
BOOST_FOREACH(const Pose3& pose, cameraPoses) {
|
|
||||||
std::cout << "Pose: " << pose << std::endl;
|
|
||||||
}
|
|
||||||
BOOST_FOREACH(const Point2& point, measured_) {
|
|
||||||
std::cout << "Point: " << point << std::endl;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
boost::optional<Point3> point;
|
boost::optional<Point3> point;
|
||||||
if (point_) {
|
try {
|
||||||
point = point_;
|
point = triangulatePoint3(cameraPoses, measured_, *K_);
|
||||||
std::cout << "Using existing point " << *point << std::endl;
|
} catch( TriangulationCheiralityException& e) {
|
||||||
} else {
|
// point is behind one of the cameras, turn factor off by setting everything to 0
|
||||||
//std::cout << "Triangulate during error calc" << std::endl;
|
//std::cout << e.what() << std::end;
|
||||||
point = triangulatePoint3(cameraPoses, measured_, *K_);
|
return 0.0;
|
||||||
}
|
}
|
||||||
if (debug) std::cout << "Result: " << *point << std::endl;
|
|
||||||
|
|
||||||
if(point)
|
for(size_t i = 0; i < measured_.size(); i++) {
|
||||||
{ // triangulation produced a good estimate of landmark position
|
Pose3 pose = cameraPoses.at(i);
|
||||||
|
PinholeCamera<CALIBRATION> camera(pose, *K_);
|
||||||
|
|
||||||
for(size_t i = 0; i < measured_.size(); i++) {
|
Point2 reprojectionError(camera.project(*point) - measured_.at(i));
|
||||||
Pose3 pose = cameraPoses.at(i);
|
overallError += noise_->distance( reprojectionError.vector() );
|
||||||
PinholeCamera<CALIBRATION> camera(pose, *K_);
|
|
||||||
|
|
||||||
Point2 reprojectionError(camera.project(*point) - measured_.at(i));
|
|
||||||
overallError += noise_->distance( reprojectionError.vector() );
|
|
||||||
}
|
|
||||||
return overallError;
|
|
||||||
} else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error
|
|
||||||
std::cout << "WARNING: Could not triangulate during error calc" << std::endl;
|
|
||||||
return 0.0;
|
|
||||||
}
|
}
|
||||||
|
return overallError;
|
||||||
} else {
|
} else {
|
||||||
return 0.0;
|
return 0.0;
|
||||||
}
|
}
|
||||||
|
|
@ -458,6 +351,16 @@ namespace gtsam {
|
||||||
return measured_;
|
return measured_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/** return the noise model */
|
||||||
|
const SharedNoiseModel& noise() const {
|
||||||
|
return noise_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** return the noise landmark */
|
||||||
|
boost::optional<Point3> point() const {
|
||||||
|
return point_;
|
||||||
|
}
|
||||||
|
|
||||||
/** return the calibration object */
|
/** return the calibration object */
|
||||||
inline const boost::shared_ptr<CALIBRATION> calibration() const {
|
inline const boost::shared_ptr<CALIBRATION> calibration() const {
|
||||||
return K_;
|
return K_;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue