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

fixed behavior of error for sigma==0

release/4.3a0
Frank Dellaert 2009-12-31 10:28:43 +00:00
parent 9b327d1a9d
commit 3fbc857a9e
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cpp/NonlinearFactor.h
View File

@ -13,6 +13,7 @@
#pragma once #pragma once
#include <list> #include <list>
#include <limits>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/serialization/list.hpp> #include <boost/serialization/list.hpp>
@ -91,6 +92,10 @@ namespace gtsam {
/** calculate the error of the factor */ /** calculate the error of the factor */
double error(const Config& c) const { double error(const Config& c) const {
if (sigma_==0.0) {
Vector e = error_vector(c);
return (inner_prod(e,e)>0) ? std::numeric_limits<double>::infinity() : 0.0;
}
Vector e = error_vector(c) / sigma_; Vector e = error_vector(c) / sigma_;
return 0.5 * inner_prod(e,e); return 0.5 * inner_prod(e,e);
}; };