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

Normalizing rotation matrix to fix numerical instability when composing many poses

release/4.3a0
Richard Roberts 2010-02-09 22:44:02 +00:00
parent 31fc894a4a
commit e83b6c3b84
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cpp/Rot2.h
View File

@ -27,6 +27,14 @@ namespace gtsam {
/** constructor from cos/sin */ /** constructor from cos/sin */
Rot2(double c, double s) : Rot2(double c, double s) :
c_(c), s_(s) { c_(c), s_(s) {
// rtodo: Could do this scale correction only when creating from compose
// Don't let scale drift
double scale = c*c + s*s;
if(scale != 1.0) {
scale = pow(scale, -0.5);
c_ *= scale;
s_ *= scale;
}
} }
/** default constructor, zero rotation */ /** default constructor, zero rotation */