From 2c15159659636594d4bc8d79b3be2eaf1725ad49 Mon Sep 17 00:00:00 2001 From: cnr-isti-vclab Date: Tue, 3 May 2011 15:19:04 +0000 Subject: [PATCH] fixed multmatrix when a scaling factor is present --- vcg/math/shot.h | 274 ++++++++++++++++++++++++------------------------ 1 file changed, 138 insertions(+), 136 deletions(-) diff --git a/vcg/math/shot.h b/vcg/math/shot.h index 0a44c8f0..2f5e221b 100644 --- a/vcg/math/shot.h +++ b/vcg/math/shot.h @@ -129,127 +129,129 @@ namespace vcg{ template > class Shot { public: - typedef Camera CameraType; - typedef S ScalarType; + typedef Camera CameraType; + typedef S ScalarType; - template - class ReferenceFrame { - friend class Shot; - RotoType rot; // rotation - Point3 tra; // viewpoint - public: - void SetIdentity(){ rot.SetIdentity(); tra = Point3(0.0,0.0,0.0);} - void SetTra(const Point3 & tr) {tra = tr;} - void SetRot(const RotoType & rt) {rot = rt;} - Point3 Tra() const { return tra;} - RotoType Rot() const { return rot;} - }; + template + class ReferenceFrame { + friend class Shot; + RotoType rot; // rotation + Point3 tra; // viewpoint + public: + void SetIdentity(){ rot.SetIdentity(); tra = Point3(0.0,0.0,0.0);} + void SetTra(const Point3 & tr) {tra = tr;} + void SetRot(const RotoType & rt) {rot = rt;} + Point3 Tra() const { return tra;} + RotoType Rot() const { return rot;} + }; - Camera Intrinsics; // the camera that made the shot - ReferenceFrame Extrinsics; // the position and orientation of the camera + Camera Intrinsics; // the camera that made the shot + ReferenceFrame Extrinsics; // the position and orientation of the camera - Shot(Camera c) - { - Intrinsics = c; - Extrinsics.SetIdentity(); - } + Shot(Camera c) + { + Intrinsics = c; + Extrinsics.SetIdentity(); + } - Shot() - { - Extrinsics.SetIdentity(); - } + Shot() + { + Extrinsics.SetIdentity(); + } - /// GET the i-th axis of the coordinate system of the camera - vcg::Point3 Axis(const int & i)const; + /// GET the i-th axis of the coordinate system of the camera + vcg::Point3 Axis(const int & i)const; /// GET the viewdir const vcg::Point3 GetViewDir()const; /// GET the viewpoint const vcg::Point3 GetViewPoint()const; /// SET the viewpoint - void SetViewPoint(const vcg::Point3 & viewpoint); + void SetViewPoint(const vcg::Point3 & viewpoint); - /// GET fov from focal - float GetFovFromFocal(); + /// GET fov from focal + float GetFovFromFocal(); - /// look at (point+up) - void LookAt(const vcg::Point3 & point,const vcg::Point3 & up); + /// look at (point+up) + void LookAt(const vcg::Point3 & point,const vcg::Point3 & up); - /// look at (opengl-like) - void LookAt(const S & eye_x,const S & eye_y,const S & eye_z, - const S & at_x,const S & at_y,const S & at_z, - const S & up_x,const S & up_y,const S & up_z); + /// look at (opengl-like) + void LookAt(const S & eye_x,const S & eye_y,const S & eye_z, + const S & at_x,const S & at_y,const S & at_z, + const S & up_x,const S & up_y,const S & up_z); - /// look towards (dir+up) - void LookTowards(const vcg::Point3 & z_dir,const vcg::Point3 & up); + /// look towards (dir+up) + void LookTowards(const vcg::Point3 & z_dir,const vcg::Point3 & up); - /// convert a 3d point from world to camera coordinates - vcg::Point3 ConvertWorldToCameraCoordinates(const vcg::Point3 & p) const; + /// convert a 3d point from world to camera coordinates + vcg::Point3 ConvertWorldToCameraCoordinates(const vcg::Point3 & p) const; - /// convert a 3d point from camera to world coordinates - vcg::Point3 ConvertCameraToWorldCoordinates(const vcg::Point3 & p) const; + /// convert a 3d point from camera to world coordinates + vcg::Point3 ConvertCameraToWorldCoordinates(const vcg::Point3 & p) const; - /// convert a 3d point from camera to world coordinates, uses inverse instead of trranspose - /// for non-exactly-rigid rotation matrices (such as calculated by tsai and garcia) - vcg::Point3 ConvertCameraToWorldCoordinates_Substitute(const vcg::Point3 & p) const; + /// convert a 3d point from camera to world coordinates, uses inverse instead of trranspose + /// for non-exactly-rigid rotation matrices (such as calculated by tsai and garcia) + vcg::Point3 ConvertCameraToWorldCoordinates_Substitute(const vcg::Point3 & p) const; - /// project a 3d point from world coordinates to 2d camera viewport (pixel) - vcg::Point2 Project(const vcg::Point3 & p) const; + /// project a 3d point from world coordinates to 2d camera viewport (pixel) + vcg::Point2 Project(const vcg::Point3 & p) const; - /// inverse projection from 2d camera viewport (pixel) + Zdepth to 3d world coordinates - vcg::Point3 UnProject(const vcg::Point2 & p, const S & d) const; + /// inverse projection from 2d camera viewport (pixel) + Zdepth to 3d world coordinates + vcg::Point3 UnProject(const vcg::Point2 & p, const S & d) const; - /// inverse projection from 2d camera viewport (pixel) + Zdepth to 3d world coordinates, uses inverse instead of trranspose - /// for non-exactly-rigid rotation matrices (such as calculated by tsai and garcia) - vcg::Point3 UnProject_Substitute(const vcg::Point2 & p, const S & d) const; + /// inverse projection from 2d camera viewport (pixel) + Zdepth to 3d world coordinates, uses inverse instead of trranspose + /// for non-exactly-rigid rotation matrices (such as calculated by tsai and garcia) + vcg::Point3 UnProject_Substitute(const vcg::Point2 & p, const S & d) const; - /// returns distance of point p from camera plane (z depth), used for unprojection - S Depth(const vcg::Point3 & p)const; + /// returns distance of point p from camera plane (z depth), used for unprojection + S Depth(const vcg::Point3 & p)const; // accessors public: - /* Returns the matrix M such that - 3dpoint_in_world_coordinates = M * 3dpoint_in_local_coordinates - */ - Matrix44 GetExtrinsicsToWorldMatrix() const { - Matrix44 rotM ; - Extrinsics.rot.ToMatrix(rotM); - return Matrix44().SetTranslate(Extrinsics.tra) * rotM.transpose(); - } + /* Returns the matrix M such that + 3dpoint_in_world_coordinates = M * 3dpoint_in_local_coordinates + */ + Matrix44 GetExtrinsicsToWorldMatrix() const { + Matrix44 rotM ; + Extrinsics.rot.ToMatrix(rotM); + return Matrix44().SetTranslate(Extrinsics.tra) * rotM.transpose(); + } - /* Returns the matrix M such that - 3dpoint_in_local_coordinates = M * 3dpoint_in_world_coordinates - */ - Matrix44 GetWorldToExtrinsicsMatrix() const { - Matrix44 rotM ; - Extrinsics.rot.ToMatrix(rotM); - return rotM * Matrix44().SetTranslate(-Extrinsics.tra) ; - } + /* Returns the matrix M such that + 3dpoint_in_local_coordinates = M * 3dpoint_in_world_coordinates + */ + Matrix44 GetWorldToExtrinsicsMatrix() const { + Matrix44 rotM ; + Extrinsics.rot.ToMatrix(rotM); + return rotM * Matrix44().SetTranslate(-Extrinsics.tra) ; + } - /* multiply the current reference frame for the matrix passed - note: it is up to the caller to check the the matrix passed is a pure rototraslation - */ + /* multiply the current reference frame for the matrix passed + note: it is up to the caller to check the the matrix passed is a pure rototraslation + the matrix can have a scaling component, but is assumed uniform over the rows + */ void MultMatrix( vcg::Matrix44 m44) { Extrinsics.tra = m44 * Extrinsics.tra; m44[0][3] = m44[1][3] = m44[2][3] = 0.0; //set no translation const S k = m44.GetRow3(0).Norm(); //compute scaling (assumed uniform) Extrinsics.rot = Extrinsics.rot * m44.transpose() * (1/k); + Extrinsics.rot.ElementAt(3,3)=S(1.0); //fix back after scaling } - /* multiply the current reference frame for the similarity passed - note: it is up to the caller to check the the matrix passed is a pure rototraslation - */ - void MultSimilarity( const Similarity & s){ MultMatrix(s.Matrix());} - - bool IsValid() const - { - return Intrinsics.PixelSizeMm[0]>0 && Intrinsics.PixelSizeMm[1]>0; - } + /* multiply the current reference frame for the similarity passed + note: it is up to the caller to check the the matrix passed is a pure rototraslation + */ + void MultSimilarity( const Similarity & s){ MultMatrix(s.Matrix());} + + bool IsValid() const + { + return Intrinsics.PixelSizeMm[0]>0 && Intrinsics.PixelSizeMm[1]>0; + } }; // end class definition @@ -268,13 +270,13 @@ const vcg::Point3 Shot::GetViewDir() const template const vcg::Point3 Shot::GetViewPoint() const { - return Extrinsics.tra; + return Extrinsics.tra; } /// SET the viewpoint template void Shot::SetViewPoint(const vcg::Point3 & viewpoint) { - Extrinsics.SetTra( viewpoint ); + Extrinsics.SetTra( viewpoint ); } //--- @@ -282,8 +284,8 @@ void Shot::SetViewPoint(const vcg::Point3 & viewpoint) template float Shot::GetFovFromFocal() { - double viewportYMm= Intrinsics.PixelSizeMm[1]* Intrinsics.ViewportPx[1]; - return 2*(vcg::math::ToDeg(atanf(viewportYMm/(2*Intrinsics.FocalMm)))); + double viewportYMm= Intrinsics.PixelSizeMm[1]* Intrinsics.ViewportPx[1]; + return 2*(vcg::math::ToDeg(atanf(viewportYMm/(2*Intrinsics.FocalMm)))); } //--- @@ -292,43 +294,43 @@ float Shot::GetFovFromFocal() template vcg::Point3 Shot::Axis(const int & i) const { - vcg::Matrix44 m; - Extrinsics.rot.ToMatrix(m); - vcg::Point3 aa = m.GetRow3(i); - return aa; + vcg::Matrix44 m; + Extrinsics.rot.ToMatrix(m); + vcg::Point3 aa = m.GetRow3(i); + return aa; } /// look at (point+up) template void Shot::LookAt(const vcg::Point3 & z_dir,const vcg::Point3 & up) { - LookTowards(z_dir-GetViewPoint(),up); + LookTowards(z_dir-GetViewPoint(),up); } /// look at (opengl-like) template void Shot::LookAt(const S & eye_x, const S & eye_y, const S & eye_z, - const S & at_x, const S & at_y, const S & at_z, - const S & up_x,const S & up_y,const S & up_z) + const S & at_x, const S & at_y, const S & at_z, + const S & up_x,const S & up_y,const S & up_z) { - SetViewPoint(Point3(eye_x,eye_y,eye_z)); - LookAt(Point3(at_x,at_y,at_z),Point3(up_x,up_y,up_z)); + SetViewPoint(Point3(eye_x,eye_y,eye_z)); + LookAt(Point3(at_x,at_y,at_z),Point3(up_x,up_y,up_z)); } /// look towards template void Shot::LookTowards(const vcg::Point3 & z_dir,const vcg::Point3 & up) { - vcg::Point3 x_dir = up ^-z_dir; - vcg::Point3 y_dir = -z_dir ^x_dir; + vcg::Point3 x_dir = up ^-z_dir; + vcg::Point3 y_dir = -z_dir ^x_dir; - Matrix44 m; - m.SetIdentity(); - *(vcg::Point3 *)&m[0][0] = x_dir/x_dir.Norm(); - *(vcg::Point3 *)&m[1][0] = y_dir/y_dir.Norm(); - *(vcg::Point3 *)&m[2][0] = -z_dir/z_dir.Norm(); + Matrix44 m; + m.SetIdentity(); + *(vcg::Point3 *)&m[0][0] = x_dir/x_dir.Norm(); + *(vcg::Point3 *)&m[1][0] = y_dir/y_dir.Norm(); + *(vcg::Point3 *)&m[2][0] = -z_dir/z_dir.Norm(); - Extrinsics.rot.FromMatrix(m); + Extrinsics.rot.FromMatrix(m); } //--- Space transformation methods @@ -337,74 +339,74 @@ void Shot::LookTowards(const vcg::Point3 & z_dir,const vcg::P template vcg::Point3 Shot::ConvertWorldToCameraCoordinates(const vcg::Point3 & p) const { - Matrix44 rotM; - Extrinsics.rot.ToMatrix(rotM); - vcg::Point3 cp = rotM * (p - GetViewPoint() ); - cp[2]=-cp[2]; // note: camera reference system is right handed - return cp; - } + Matrix44 rotM; + Extrinsics.rot.ToMatrix(rotM); + vcg::Point3 cp = rotM * (p - GetViewPoint() ); + cp[2]=-cp[2]; // note: camera reference system is right handed + return cp; + } /// convert a 3d point from camera to world coordinates template vcg::Point3 Shot::ConvertCameraToWorldCoordinates(const vcg::Point3 & p) const { - Matrix44 rotM; - vcg::Point3 cp = p; - cp[2]=-cp[2]; // note: World reference system is left handed - Extrinsics.rot.ToMatrix(rotM); - cp = rotM.transpose() * cp + GetViewPoint(); - return cp; + Matrix44 rotM; + vcg::Point3 cp = p; + cp[2]=-cp[2]; // note: World reference system is left handed + Extrinsics.rot.ToMatrix(rotM); + cp = rotM.transpose() * cp + GetViewPoint(); + return cp; } -/// convert a 3d point from camera to world coordinates, uses inverse instead of trranspose +/// convert a 3d point from camera to world coordinates, uses inverse instead of trranspose /// for non-exactly-rigid rotation matrices (such as calculated by tsai and garcia) template vcg::Point3 Shot::ConvertCameraToWorldCoordinates_Substitute(const vcg::Point3 & p) const { - Matrix44 rotM; - vcg::Point3 cp = p; - cp[2]=-cp[2]; // note: World reference system is left handed - Extrinsics.rot.ToMatrix(rotM); - cp = Inverse(rotM) * cp + GetViewPoint(); // use invert istead of transpose to dela with non-rigid cases - return cp; + Matrix44 rotM; + vcg::Point3 cp = p; + cp[2]=-cp[2]; // note: World reference system is left handed + Extrinsics.rot.ToMatrix(rotM); + cp = Inverse(rotM) * cp + GetViewPoint(); // use invert istead of transpose to dela with non-rigid cases + return cp; } /// project a 3d point from world coordinates to 2d camera viewport (pixel) template vcg::Point2 Shot::Project(const vcg::Point3 & p) const { - Point3 cp = ConvertWorldToCameraCoordinates(p); - Point2 pp = Intrinsics.Project(cp); - Point2 vp = Intrinsics.LocalToViewportPx(pp); - return vp; + Point3 cp = ConvertWorldToCameraCoordinates(p); + Point2 pp = Intrinsics.Project(cp); + Point2 vp = Intrinsics.LocalToViewportPx(pp); + return vp; } /// inverse projection from 2d camera viewport (pixel) + Zdepth to 3d world coordinates template vcg::Point3 Shot::UnProject(const vcg::Point2 & p, const S & d) const { - Point2 lp = Intrinsics.ViewportPxToLocal(p); - Point3 cp = Intrinsics.UnProject(lp,d); - Point3 wp = ConvertCameraToWorldCoordinates(cp); - return wp; + Point2 lp = Intrinsics.ViewportPxToLocal(p); + Point3 cp = Intrinsics.UnProject(lp,d); + Point3 wp = ConvertCameraToWorldCoordinates(cp); + return wp; } -/// inverse projection from 2d camera viewport (pixel) + Zdepth to 3d world coordinates, uses inverse instead of trranspose +/// inverse projection from 2d camera viewport (pixel) + Zdepth to 3d world coordinates, uses inverse instead of trranspose /// for non-exactly-rigid rotation matrices (such as calculated by tsai and garcia) template vcg::Point3 Shot::UnProject_Substitute(const vcg::Point2 & p, const S & d) const { - Point2 lp = Intrinsics.ViewportPxToLocal(p); - Point3 cp = Intrinsics.UnProject(lp,d); - Point3 wp = ConvertCameraToWorldCoordinates_Substitute(cp); - return wp; + Point2 lp = Intrinsics.ViewportPxToLocal(p); + Point3 cp = Intrinsics.UnProject(lp,d); + Point3 wp = ConvertCameraToWorldCoordinates_Substitute(cp); + return wp; } /// returns distance of point p from camera plane (z depth), used for unprojection template S Shot::Depth(const vcg::Point3 & p)const { - return ConvertWorldToCameraCoordinates(p).Z(); + return ConvertWorldToCameraCoordinates(p).Z(); }