diff options
| author | Martin Miller | 2017-03-22 15:47:00 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-03-22 15:47:00 -0500 |
| commit | 50834b4ba0904a13242476cc515f85fda4f30844 (patch) | |
| tree | 4f8fa5f22710482d5b9df394185921ad4889a44c /src/camera.cpp | |
| parent | d020ec03598b90fb8d78194554471b1cfb4794dd (diff) | |
| download | refslam-50834b4ba0904a13242476cc515f85fda4f30844.zip refslam-50834b4ba0904a13242476cc515f85fda4f30844.tar.gz | |
Add feature handling to state
Diffstat (limited to 'src/camera.cpp')
| -rw-r--r-- | src/camera.cpp | 11 |
1 files changed, 11 insertions, 0 deletions
diff --git a/src/camera.cpp b/src/camera.cpp index dcc5a44..b698925 100644 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -90,3 +90,14 @@ Camera::d ( ) return _d; } /* ----- end of method Camera::d ----- */ +Vector3d +Camera::img2body ( Vector3d &xi ) +{ + Vector3d xc, xb; + xc = K().inverse()*xi; + // Normalize + xc /= xc.norm(); + xb = Rc2b()*xc; + return xb; +} /* ----- end of method Camera::img2body ----- */ + |